
/**
  ******************************************************************************
  * @file           : pid.c
	* @author         :jiang kaiyue
  * @brief          :
  ******************************************************************************
  * @attention

  *
  ******************************************************************************
  */
	#include "pid.h"
#include "stdio.h"
	void PID_Init(PID_t *pid,float kp,float ki,float kd)
	{
	pid->kp=kp;
		pid->ki=ki;
		pid->kd=kd;
	}
	void PID_Initt(PID_tt *pid,float kpt,float kit,float kdt)
	{
	pid->kpt=kpt;
		pid->kit=kit;
		pid->kdt=kdt;
	}
	void PID_Inittt(PID_ttt *pid,float kptt,float kitt,float kdt)
	{
	pid->kptt=kptt;
		pid->kitt=kitt;
		pid->kdtt=kdt;
	
	}
	float PID_Calculate(PID_t *pid,float ref,float meas,float dt)
	{
		
		
    pid->Error=ref-meas ;
		
		pid->sum = pid->sum +(pid->Error) ;
		pid->output = (pid->kp)*(pid->Error)+(pid->ki)*pid->sum*dt-(pid->kd)*((pid->Error)-(pid->LastError))/dt;
		pid->LastError =ref -meas ;
			printf("c=%f,",pid->kp );
		printf("a=%f,",pid->ki  );
		printf("b=%f,",pid->kd  );

		
		
	return pid->output ;
	}
	float PID_Calculatet(PID_tt *pidt,float reft,float meast,float dt)
	{
		
		
    pidt->Errort=reft-meast ;
//		float t;
//		int k=1;
//		for(k=1;k<2;k++)
//		{
//		 t=pidt->kpt;
//		}
//	
//    pidt->Errort=reft-meast ;
//		if(pidt->Errort>2&&pidt->Errort<-2)
//		{
//			float  q = pidt->kpt*pidt->kpt;
//		pidt->kpt = q;
//		
//		}
//		else
//		{
//			pidt->kpt=t;
//		}
		pidt->sumt = pidt->sumt +(pidt->Errort) ;
		pidt->outputt = (pidt->kpt)*(pidt->Errort)+(pidt->kit)*pidt->sumt*dt+(pidt->kdt)*((pidt->Errort)-(pidt->LastErrort))/dt;
		pidt->LastErrort =reft -meast ;
		
		
		
	return pidt->outputt ;
	}
	float PID_Calculatett(PID_ttt *pidtt,float reftt,float meastt,float dt)
	{
	
		
//		static int  k =0;
//		static float   t =0;
//		for(;k<1;k++)
//		{
//		 t=pidtt->kptt;
//		}
//	
   pidtt->Errortt=reftt-meastt ;
//		if(pidtt->Errortt>2)
//		{
//			float  q = t*2;
//		pidtt->kptt = q;
//		
//		}
//		
//		else if(pidtt->Errortt<-2)
//		{
//			float  q = t*2;
//		pidtt->kptt = q;
//		
//		}
//		else
//		{
//			pidtt->kptt=t;
//		}

		
		pidtt->sumtt = pidtt->sumtt +(pidtt->Errortt) ;
		pidtt->outputtt = (pidtt->kptt)*(pidtt->Errortt)+(pidtt->kitt)*pidtt->sumtt*dt-(pidtt->kdtt)*((pidtt->Errortt)-(pidtt->LastErrortt))/dt;
		pidtt->LastErrortt =reftt -meastt ;
		
		
//	if(pidtt->Errortt<2&&pidtt->Errortt>1.5)
//		{
//		pidtt->outputtt=pidtt->outputtt*pidtt->Errortt;
//		
//		}
		
	return pidtt->outputtt ;
	}
